#ifndef PLANE_HEADER_H
#define PLANE_HEADER_H
#include <Arduino_BuiltIn.h>
#include <Arduino.h>
#include <Wire.h>
#include "BluetoothSerial.h"
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"

#define PWM_LB 100 //150
#define PWM_UB 255 //255
#define E_RP_B 10
#define E_Y_B 0.5
#define EMERGENCY_THRESHOLD 80
#define INTERVAL 1000 //单次试飞时间间隔，PID调整一次的最小时间单位
#define tau_inverse 125 // 微分时间倒数，经过实测，采样一千次，8000ms

extern const int ledPin ;
extern const int AD_pin ;
extern const int LED1_pin;
extern const int LED2_pin;
extern const int LED3_pin;
extern const int LED4_pin;
extern const int MPU_INT_pin;

extern const int PWM1_pin;
extern const int PWM2_pin;
extern const int PWM3_pin;
extern const int PWM4_pin;
 
extern const int sda_pin ;
extern const int scl_pin ;

extern const int freq           ;//PWM频率
extern const int resolution     ;//PWM分辨率
extern BluetoothSerial SerialBT;
// 角度环
extern float P_roll  ;
extern float I_roll  ;
extern float D_roll  ;//15

extern float P_pitch   ;
extern float I_pitch   ;
extern float D_pitch   ;//15

extern float P_yaw ;
extern float I_yaw ;
extern float D_yaw ;//16

// 角速度环
extern float P_roll_w ;
extern float I_roll_w ;
extern float D_roll_w ;

extern float P_pitch_w ;
extern float I_pitch_w ;
extern float D_pitch_w ;

extern float P_yaw_w ;
extern float I_yaw_w ;
extern float D_yaw_w ;

extern float agx;
extern float agy;
extern float agz;

// 全局迭代变量
extern int counter;
extern float y_log[INTERVAL], p_log[INTERVAL], r_log[INTERVAL];

extern int signal_PWM_1 ;//电机控制信号，取值范围0~255
extern int signal_PWM_2 ;
extern int signal_PWM_3 ;
extern int signal_PWM_4 ;

extern float signal_first_PWM_1;//电机初始控制信号，取值范围0~255，220起飞临界
extern float signal_first_PWM_2;
extern float signal_first_PWM_3;
extern float signal_first_PWM_4;

extern MPU6050 mpu1;
extern uint8_t fifoBuffer[128];
extern Quaternion q;
extern VectorFloat gravity;
extern float ypr[3];
extern float pitch_zero;
extern float roll_zero;
extern float ypr_target[3];
extern int16_t gyro_int[3];

// 函数声明
void Motor_Drive(int signal_PWM_1, int signal_PWM_2, int signal_PWM_3, int signal_PWM_4);
void Preheat();
void Emergency_Check(float ypr[3]);
float* calculate_pid_omega(int16_t xyz[3], float target[3]);
void calculate_pid(float ypr[3], float ypr_target[3], int16_t gyro[3]);
void UART_process();
void change_PID(String& command);
void ud_command(String& command);
void print_pid();

#endif